/*
 * @Author: junhuizhou
 * @Date: 2020-12-30 10:58:59
 * @LastEditor: junhuizhou
 * @LastEditTime: 2020-12-30 11:01:18
 * @Description: header
 * @FilePath: \fzhtq\src\calmatrix.c
 */

#include <stdio.h>
#include <stdlib.h>
#include "math.h"
#include "../include/calmatrix.h"

void MatCpy(double *pMat0, double *pMat1, int row, int col)
{
    int i, j;

    for(i=0; i<row; i++)
    {
        for(j=0; j<col; j++)
        {
            *(pMat0 + i*col + j) = *(pMat1 + i*col + j);
        }
    }
}

void MatAdd(double *pMat0, double *pMat1, double *pMat2, int row, int col)
{
    int i, j;

    for(i=0; i<row; i++)
    {
        for(j=0; j<col; j++)
        {
            *(pMat0 + i*col + j) = (*(pMat1 + i*col + j)) + (*(pMat2 + i*col + j));
        }
    }
}

void MatSub(double *pMat0, double *pMat1, double *pMat2, int row, int col)
{
    int i, j;

    for(i=0; i<row; i++)
    {
        for(j=0; j<col; j++)
        {
            *(pMat0 + i*col + j) = (*(pMat1 + i*col + j)) - (*(pMat2 + i*col + j));
        }
    }
}

void MatMtp(double *pMat0, double *pMat1, int row1, int col1, double *pMat2, int row2, int col2)
{
    int i, j, k;

    /*错误情况*/
    if((row1<1) || (col1<1) || (row2<1) || (col2<1))
    {
        return;
    }
    /*数乘情况*/
    if((row1==1) && (col1==1))
    {
        for(i=0; i<row2; i++)
        {
            for(j=0; j<col2; j++)
            {
                *(pMat0 + i*col2 + j) = (*(pMat1)) * (*(pMat2 + i*col2 + j));
            }
        }
        return;
    }
    if((row2==1) && (col2==1))
    {
        for(i=0; i<row1; i++)
        {
            for(j=0; j<col1; j++)
            {
                *(pMat0 + i*col1 + j) = (*(pMat1 + i*col1 + j)) * (*(pMat2));
            }
        }
        return;
    }
    /*一般情况*/
    if(col1 == row2)
    {
        for(i=0; i<row1; i++)
        {
            for(j=0; j<col2; j++)
            {
                *(pMat0 + i*col2 + j) = 0;
                for(k=0; k<row2; k++)
                {
                    *(pMat0 + i*col2 + j) += (*(pMat1 + i*col1 + k)) * (*(pMat2 + k*col2 + j));
                }
            }
        }
        return;
    }
    else
    {
        return;
    }
}

void MatTrans(double *pMat0, double *pMat1, int row, int col)
{
    int i, j;

    if((row<1) || (col<1))
    {
        return;
    }
    else
    {
        for(i=0; i<row; i++)
        {
            for(j=0; j<col; j++)
            {
                *(pMat0 + i*col + j) = *(pMat1 +j*row + i);
            }
        }
    }
}

void MatInv(double *pMat, int row)
{
    int* is;
    int* js;
    int i, j, k, l, u, v;
    double d, p;

    is = (int*)malloc(row*sizeof(int));
    js = (int*)malloc(row*sizeof(int));
    for(k=0;k<=row-1;k++)
    { 
    	d = 0.0;
        for(i=k; i<=row-1; i++)
        {
            for(j=k; j<=row-1; j++)
            { 
                l = i*row + j;
                p = fabs(*(pMat + l));
                if(p > d)
                { d = p; is[k] = i; js[k] = j;}
            }
        } 
        if(fabs(d) < (1e-6))
        { 
            free(is);
            free(js);  
            return;
        }
        if(is[k] != k)
          for(j=0; j<=row-1; j++)
          { 
              u = k*row + j;
              v = is[k]*row + j;
              p = *(pMat + u);
              *(pMat + u) = *(pMat + v);
              *(pMat + v) = p;
          }
        if(js[k] != k)
        {
            for(i=0; i<=row-1; i++)
            { 
                u = i*row + k;
                v = i*row + js[k];
                p = *(pMat + u);
                *(pMat + u) = *(pMat + v);
                *(pMat + v) = p;
            }
        }
        l = k*row + k;
        *(pMat + l) = 1.0 / (*(pMat + l));
        for(j=0; j<=row-1; j++)
        {
            if(j != k)
            { 
                u = k*row + j;
                *(pMat + u) = (*(pMat + u)) * (*(pMat + l));
            }
        }
        for(i=0; i<=row-1; i++)
        {
            if(i != k)
            {
                for(j=0; j<=row-1; j++)
                {
                    if(j != k)
                    { 
                        u = i*row + j;
                        *(pMat + u) = (*(pMat+u)) - (*(pMat+i*row+k))*(*(pMat+k*row+j));
                    }
                }
            }
        }
        for(i=0; i<=row-1; i++)
        {
            if(i != k)
            {
                u = i*row + k;
                *(pMat + u) = -(*(pMat + u)) * (*(pMat + l));
            }
        }
    }
    for(k=row-1; k>=0; k--)
    { 
    	if(js[k] != k)
        {
            for(j=0; j<=row-1; j++)
            { 
                u = k*row + j;
                v = js[k]*row + j;
                p = *(pMat + u);
                *(pMat + u) = *(pMat + v);
                *(pMat + v) = p;
            }
        }
        if(is[k] != k)
        {
            for(i=0; i<=row-1; i++)
            { 
                u = i*row + k;
                v = i*row + is[k];
                p = *(pMat + u);
                *(pMat + u) = *(pMat + v);
                *(pMat + v) = p;
            }
        }
    }

    free(is);
    free(js);
}

double VectorNorm(double *pMat, int count)
{
    int i;
    double tmp = 0;

    for(i=0; i<count; i++)
    {
        tmp += (*(pMat + i)) * (*(pMat + i));
    }
    tmp = sqrt(tmp);

    return tmp;
}

void VectorCross(double *pMat0, double *pMat1, double *pMat2)
{
    double Matrix[3][3];

    /*将pMat1化成斜对称矩阵*/
    Matrix[0][0] = 0;
    Matrix[0][1] = -( *(pMat1 + 2) );
    Matrix[1][0] = ( *(pMat1 + 2) );
    Matrix[1][1] = 0;
    Matrix[0][2] = ( *(pMat1 + 1) );
    Matrix[2][0] = -( *(pMat1 + 1) );
    Matrix[1][2] = -( *pMat1 );
    Matrix[2][1] = ( *pMat1 );
    Matrix[2][2] = 0;
    /*矩阵乘法*/
    MatMtp(pMat0, (double*)Matrix, 3, 3, pMat2, 3, 1);
}

void MatPrint(double *pMat, int row, int col)
{
    int i, j;

    for(i=0; i<row; i++)
    {
        for(j=0; j<col; j++)
        {
            printf("%f ", *(pMat + i*col + j));
        }
        printf("\n");
    }
}

double RandN()
{
    int i;
    double result = 0;

    for(i=0; i<12; i++)
    {
        /*fmod(x,y)求x/y整除后的双精度余数*/
        result += fmod(rand(),360)/359.0;
    }
    result -= 6;
    
    return result;
}

double Rand01()
{
    double result;
    result = fmod(rand(), 360)/359.0;
    return result;
}
